{
  "metadata": {
    "kernelspec": {
      "display_name": "Python 3",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "mimetype": "text/x-python",
      "version": "3.5.2",
      "pygments_lexer": "ipython3",
      "file_extension": ".py",
      "codemirror_mode": {
        "version": 3,
        "name": "ipython"
      },
      "nbconvert_exporter": "python",
      "name": "python"
    }
  },
  "nbformat_minor": 0,
  "nbformat": 4,
  "cells": [
    {
      "execution_count": null,
      "source": [
        "%matplotlib inline"
      ],
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      }
    },
    {
      "metadata": {},
      "source": [
        "\n********************************************************************************\n2D Robot Localization - Benchmark\n********************************************************************************\nGoals of this script:\n\n- implement different UKFs on the 2D robot localization example.\n\n- design the Extended Kalman Filter (EKF) and the Invariant Extended Kalman\n  Filter (IEKF) :cite:`barrauInvariant2017`.\n\n- compare the different algorithms with Monte-Carlo simulations.\n\n*We assume the reader is already familiar with the considered problem described\nin the tutorial.*\n\nWe previously designed an UKF with a standard uncertainty representation. An\nadvantage of the versatility of the UKF is to speed up implementation, tests,\nand comparision of algorithms with different uncertainty representations.\nIndeed, for the given problem, three different UKFs emerge, defined respectively\nas:\n\n1) The state is embedded in $SO(2) \\times \\mathbb{R}^2$, where:\n\n   * the retraction $\\varphi(.,.)$ is the $SO(2)$ exponential\n     for orientation and the vector addition for position.\n\n   * the inverse retraction $\\varphi^{-1}(.,.)$ is the $SO(2)$\n     logarithm for orientation and the vector subtraction for position.\n\n2) The state is embedded in $SE(2)$ with left multiplication, i.e.\n\n   - the retraction $\\varphi(.,.)$ is the $SE(2)$ exponential,\n     where the state multiplies on the left the uncertainty\n     $\\boldsymbol{\\xi}$.\n\n   - the inverse retraction $\\varphi^{-1}(.,.)$ is the $SE(2)$\n     logarithm.\n\n   - this left UKF on $SE(2)$ corresponds to the Invariant Extended Kalman\n     Filter (IEKF) recommended in :cite:`barrauInvariant2017`. \n\n3) The state is embedded in $SE(2)$ with right multiplication, i.e.\n\n   - the retraction $\\varphi(.,.)$ is the $SE(2)$ exponential,\n     where the state multiplies on the right the uncertainty\n     $\\boldsymbol{\\xi}$.\n\n   - the inverse retraction $\\varphi^{-1}(.,.)$ is the $SE(2)$\n     logarithm.\n\nWe tests the filters on simulation with strong initial heading error.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "metadata": {},
      "source": [
        "Import\n==============================================================================\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "execution_count": null,
      "source": [
        "from ukfm import SO2, UKF, EKF\nfrom ukfm import LOCALIZATION as MODEL\nimport ukfm\nimport numpy as np\nimport matplotlib\nukfm.utils.set_matplotlib_config()"
      ],
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      }
    },
    {
      "metadata": {},
      "source": [
        "We compare the filters on a large number of Monte-Carlo runs.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "execution_count": null,
      "source": [
        "# Monte-Carlo runs\nN_mc = 100"
      ],
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      }
    },
    {
      "metadata": {},
      "source": [
        "Simulation Setting\n==============================================================================\nWe set the simulation as in :cite:`barrauInvariant2017`, section IV. The robot\ndrives along a 10 m diameter circle for 40 seconds with high rate odometer\nmeasurements (100 Hz) and low rate GPS measurements (1 Hz). The vehicle gets\nmoderate angular velocity uncertainty and highly precise linear velocity. The\ninitial values of the heading error is very strong, **45\u00b0 standard\ndeviation**, while the initial position is known.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "execution_count": null,
      "source": [
        "# sequence time (s)\nT = 40\n# odometry frequency (Hz)\nodo_freq = 100\n# create the model\nmodel = MODEL(T, odo_freq)\n# odometry noise standard deviation\nodo_std = np.array([0.01,  # speed (v/m)\n                    0.01,  # speed (v/m)\n                    1 / 180 * np.pi])  # angular speed (rad/s)\n# GPS frequency (Hz)\ngps_freq = 1\n# GPS noise standard deviation (m)\ngps_std = 1\n# radius of the circle trajectory (m)\nradius = 5\n# initial heading error standard deviation\ntheta0_std = 45/180*np.pi"
      ],
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      }
    },
    {
      "metadata": {},
      "source": [
        "Filter Design\n==============================================================================\nThe UKFs are compared to an Extended Kalman FIlter (EKF) and an Invariant EKF\n(IEKF). The EKF has the same uncertainty representation as the UKF with the\nretraction on $SO(2) \\times \\mathbb{R}^2$, whereas the IEKF has the same\nuncertainty representation as the UKF with the left retraction on\n$SE(2)$.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "execution_count": null,
      "source": [
        "# propagation noise covariance matrix\nQ = np.diag(odo_std**2)\n# measurement noise covariance matrix\nR = gps_std**2*np.eye(2)\n# initial covariance matrix\nP0 = np.zeros((3, 3))\n# we take into account initial heading error\nP0[0, 0] = theta0_std ** 2\n# sigma point parameter\nalpha = np.array([1e-3, 1e-3, 1e-3])"
      ],
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      }
    },
    {
      "metadata": {},
      "source": [
        "We set error variables before launching Monte-Carlo simulations. As we have\nfive similar methods, the code is redundant.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "execution_count": null,
      "source": [
        "ukf_err = np.zeros((N_mc, model.N, 3))\nleft_ukf_err = np.zeros_like(ukf_err)\nright_ukf_err = np.zeros_like(ukf_err)\niekf_err = np.zeros_like(ukf_err)\nekf_err = np.zeros_like(ukf_err)"
      ],
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      }
    },
    {
      "metadata": {},
      "source": [
        "We record Normalized Estimation Error Squared (NEES) for consistency\nevaluation (see Results).\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "execution_count": null,
      "source": [
        "ukf_nees = np.zeros((N_mc, model.N, 2))\nleft_ukf_nees = np.zeros_like(ukf_nees)\nright_ukf_nees = np.zeros_like(ukf_nees)\niekf_nees = np.zeros_like(ukf_nees)\nekf_nees = np.zeros_like(ukf_nees)"
      ],
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      }
    },
    {
      "metadata": {},
      "source": [
        "Monte-Carlo Runs\n==============================================================================\nWe run the Monte-Carlo through a for loop.\n\n<div class=\"alert alert-info\"><h4>Note</h4><p>We sample for each Monte-Carlo run an initial heading error from the true\n    distribution ($\\mathbf{P}_0$). This requires many Monte-Carlo\n    samples.</p></div>\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "execution_count": null,
      "source": [
        "for n_mc in range(N_mc):\n    print(\"Monte-Carlo iteration(s): \" + str(n_mc + 1) + \"/\" + str(N_mc))\n    # simulation true trajectory\n    states, omegas = model.simu_f(odo_std, radius)\n    # simulate measurement\n    ys, one_hot_ys = model.simu_h(states, gps_freq, gps_std)\n    # initialize filter with inaccurate state\n    state0 = model.STATE(\n        Rot=states[0].Rot.dot(SO2.exp(theta0_std * np.random.randn(1))),\n        p=states[0].p)\n    # define the filters\n    ukf = UKF(state0=state0, P0=P0, f=model.f, h=model.h, Q=Q, R=R,\n              phi=model.phi,\n              phi_inv=model.phi_inv,\n              alpha=alpha)\n    left_ukf = UKF(state0=state0, P0=P0, f=model.f, h=model.h, Q=Q, R=R,\n                   phi=model.left_phi,\n                   phi_inv=model.left_phi_inv,\n                   alpha=alpha)\n    right_ukf = UKF(state0=state0, P0=P0, f=model.f, h=model.h, Q=Q, R=R,\n                    phi=model.right_phi,\n                    phi_inv=model.right_phi_inv,\n                    alpha=alpha)\n    iekf = EKF(model=model, state0=state0, P0=P0, Q=Q, R=R,\n               FG_ana=model.iekf_FG_ana,\n               H_ana=model.iekf_H_ana,\n               phi=model.left_phi)\n    ekf = EKF(model=model, state0=state0, P0=P0, Q=Q, R=R,\n              FG_ana=model.ekf_FG_ana,\n              H_ana=model.ekf_H_ana,\n              phi=model.phi)\n    # variables for recording estimates of the Monte-Carlo run\n    ukf_states = [state0]\n    left_states = [state0]\n    right_states = [state0]\n    iekf_states = [state0]\n    ekf_states = [state0]\n\n    ukf_Ps = np.zeros((model.N, 3, 3))\n    left_ukf_Ps = np.zeros_like(ukf_Ps)\n    right_ukf_Ps = np.zeros_like(ukf_Ps)\n    ekf_Ps = np.zeros_like(ukf_Ps)\n    iekf_Ps = np.zeros_like(ukf_Ps)\n\n    ukf_Ps[0] = P0\n    left_ukf_Ps[0] = P0\n    right_ukf_Ps[0] = P0\n    ekf_Ps[0] = P0\n    iekf_Ps[0] = P0\n\n    # measurement iteration number\n    k = 1\n\n    # filtering loop\n    for n in range(1, model.N):\n        ukf.propagation(omegas[n-1], model.dt)\n        left_ukf.propagation(omegas[n-1], model.dt)\n        right_ukf.propagation(omegas[n-1], model.dt)\n        iekf.propagation(omegas[n-1], model.dt)\n        ekf.propagation(omegas[n-1], model.dt)\n        # update only if a measurement is received\n        if one_hot_ys[n] == 1:\n            ukf.update(ys[k])\n            left_ukf.update(ys[k])\n            right_ukf.update(ys[k])\n            iekf.update(ys[k])\n            ekf.update(ys[k])\n            k = k + 1\n        ukf_states.append(ukf.state)\n        left_states.append(left_ukf.state)\n        right_states.append(right_ukf.state)\n        iekf_states.append(iekf.state)\n        ekf_states.append(ekf.state)\n\n        ukf_Ps[n] = ukf.P\n        left_ukf_Ps[n] = left_ukf.P\n        right_ukf_Ps[n] = right_ukf.P\n        iekf_Ps[n] = iekf.P\n        ekf_Ps[n] = ekf.P\n\n    # get state trajectory\n    Rots, ps = model.get_states(states, model.N)\n    ukf_Rots, ukf_ps = model.get_states(ukf_states, model.N)\n    left_ukf_Rots, left_ukf_ps = model.get_states(left_states, model.N)\n    right_ukf_Rots, right_ukf_ps = model.get_states(right_states, model.N)\n    iekf_Rots, iekf_ps = model.get_states(iekf_states, model.N)\n    ekf_Rots, ekf_ps = model.get_states(ekf_states, model.N)\n\n    # record errors\n    ukf_err[n_mc] = model.errors(Rots, ukf_Rots, ps, ukf_ps)\n    left_ukf_err[n_mc] = model.errors(Rots, left_ukf_Rots, ps, left_ukf_ps)\n    right_ukf_err[n_mc] = model.errors(Rots, right_ukf_Rots, ps, right_ukf_ps)\n    iekf_err[n_mc] = model.errors(Rots, iekf_Rots, ps, iekf_ps)\n    ekf_err[n_mc] = model.errors(Rots, ekf_Rots, ps, ekf_ps)\n\n    # record NEES\n    ukf_nees[n_mc] = model.nees(ukf_err[n_mc], ukf_Ps, ukf_Rots, ukf_ps, 'STD')\n    left_ukf_nees[n_mc] = model.nees(left_ukf_err[n_mc], left_ukf_Ps,\n                                     left_ukf_Rots, left_ukf_ps, 'LEFT')\n    right_ukf_nees[n_mc] = model.nees(right_ukf_err[n_mc], right_ukf_Ps,\n                                      right_ukf_Rots, right_ukf_ps, 'RIGHT')\n    iekf_nees[n_mc] = model.nees(iekf_err[n_mc], iekf_Ps, iekf_Rots, iekf_ps,\n                                 'LEFT')\n    ekf_nees[n_mc] = model.nees(ekf_err[n_mc], ekf_Ps, ekf_Rots, ekf_ps, 'STD')"
      ],
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      }
    },
    {
      "metadata": {},
      "source": [
        "Results\n==============================================================================\nWe first visualize the robot trajectory (for the last run) and the errors\nw.r.t. orientation and position (averaged over Monte-Carlo). As simulations\nhave random process, the trajectory plot just gives us an indication but not a\nproof of performances.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "execution_count": null,
      "source": [
        "ukf_e, left_ukf_e, right_ukf_e, iekf_e, ekf_e = model.benchmark_plot(\n    ukf_err, left_ukf_err, right_ukf_err, iekf_err, ekf_err, ps, ukf_ps,\n    left_ukf_ps, right_ukf_ps, ekf_ps, iekf_ps)"
      ],
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      }
    },
    {
      "metadata": {},
      "source": [
        "Two groups of filters emerge: group 1) consists of EKF and $SO(2) \\times\n\\mathbb{R}^2$ UKF; and group 2) have IEKF, left $SE(2)$ UKF and right\n$SE(2)$ UKF (the curves of these filters are superposed). The second\ngroup is visibly highly better regarding position estimation.\n\nMore statictical is to compute the results averaged over all the Monte-Carlo.\nLet us compute the Root Mean Squared Error (RMSE) for each method both for the\norientation and the position.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "execution_count": null,
      "source": [
        "model.benchmark_print(ukf_e, left_ukf_e, right_ukf_e, iekf_e, ekf_e)"
      ],
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      }
    },
    {
      "metadata": {},
      "source": [
        "They confirm the results on the plot.\n\nA consistency metric is the Normalized Estimation Error Squared (NEES).\nClassical criteria used to evaluate the performance of an estimation method,\nlike the RMSE, do not inform about consistency as they do not take into\naccount the uncertainty returned by the filter. This point is addressed by the\nNEES, which computes the average squared value of the error, normalized by the\ncovariance matrix of the filter. The case NEES>1 reveals an inconsistency\nissue: the actual uncertainty is higher than the computed uncertainty.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "execution_count": null,
      "source": [
        "model.nees_print(ukf_nees, left_ukf_nees, right_ukf_nees, iekf_nees, ekf_nees)"
      ],
      "outputs": [],
      "cell_type": "code",
      "metadata": {
        "collapsed": false
      }
    },
    {
      "metadata": {},
      "source": [
        "As the filters are initialized with perfect position and zero covariance\nw.r.t. position, we compute NEES only after 20 s for avoiding numerical issues\n(during the first secondes of the trajectory the covariance matrix\n$\\mathbf{P}_n$ is very low so inverting it leads to insignificantly high\nnumbers). Results are clear, the $SE(2)$ UKF are the more consistent.\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "metadata": {},
      "source": [
        "**Which filter is the best ?** In this setting, the **left UKF**, the\n**right UKF** and the IEKF filters obtain similar accurate results, that\nclearly outperform $SO(2) \\times \\mathbb{R}^2$ UKF, and EKF, whereas the\ntwo UKFs are the more consistent.\n\n<div class=\"alert alert-info\"><h4>Note</h4><p>We have set all the filters with the same \"true\" noise covariance\n   parameters. However, both EKF and UKF based algorithms may better deal  ,\n   with non-linearity  by e.g. inflated propagation noise covariance.</p></div>\n\n\n"
      ],
      "cell_type": "markdown"
    },
    {
      "metadata": {},
      "source": [
        "Conclusion\n==============================================================================\nThis script compares different algorithms for 2D robot localization. Two\ngroups of filters emerge: the $SO(2) \\times \\mathbb{R}^2$ UKF and the\nEKF represent the first group; and the left $SE(2)$ UKF, the right\n$SE(2)$ UKF and the IEKF constitute the second group. For the considered\nset of parameters, it is evident that embedded the state in $SE(2)$ is\nadvantageous for state estimation.\n\nYou can now:\n\n* compare the filters in different scenarios. Indeed, UKF and their (I)EKF\n  counterparts may obtain different results when noise is e.g. inflated or\n  with different initial conditions or different trajectory.\n\n* test the filters in a slightly different model (e.g. with orientation\n  measurement), which is straightforward for the UKFs.\n\n"
      ],
      "cell_type": "markdown"
    }
  ]
}